#include <stdlib.h>
#include <time.h>
#include <stdio.h>

#define _VIDEO_AVI_CPP_

#include "stereo.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "basic_utils.hh"
#include "cvfundam2.h"
#include "other_functions.h"

CvFont font;
int initfont=0;
char windowgraph[][100] = {"Window 0", "Window 1"};

int do_Estereo(IplImage **image, 
	       CvMat **pointsI, CvMat **pointsD, CvMat **points3D, CvMat **angY,
	       CALIBRATION_PARAMS_t * calL, CALIBRATION_PARAMS_t* calR, struct corners_params_t *cp,
	       bool graficar)
{   
    IplImage *grey[2];
    int i;
    CvRect haar_rect[2];
    double hScale=0.47, vScale = 0.47;
    int lineWidth = 1;
    HaarDetector haar;
    CvRect haar_rect2[2];
    int npoints;

    haar.load(NULL);
    haar.set_debug(false);

    if (!initfont)
    {
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth);
	initfont=1;
    }

    //return 0;

    for (i=0; i<2; i++)
    {
	grey[i] = cvCreateImage( cvGetSize(image[i]), 8, 1 );
	if(image[i]->nChannels!=1)
	    cvCvtColor( image[i], grey[i], CV_BGR2GRAY );
	else
	    cvCopy(image[i], grey[i]);

	//if (b>0)
	//    cvFlip(image[i], NULL, 0);
	haar_rect[i] = 	haar.detect(image[i], HAAR_MIN_SIZE, false);

	float quitar_x = 1/6.0f;
	float quitar_y = 0;////1/8.0f;
	haar_rect2[i] = cvRect(haar_rect[i].x + (int)(haar_rect[i].width * quitar_x), 
			       haar_rect[i].y + (int)(haar_rect[i].height * quitar_y), 
			       (int)(haar_rect[i].width* (1-2*quitar_x)), 
			       (int)(haar_rect[i].height* (1-2*quitar_y)));
    }


    if ((haar_rect[0].x==0 && haar_rect[0].y == 0) || (haar_rect[1].x == 0 && haar_rect[1].y == 0))
    {
	//no se encontró la cara. Se pasa el frame
	cvReleaseImage(&grey[0]);
	cvReleaseImage(&grey[1]);

	return 0;
    }

    npoints = 25;
    CvPoint2D32f * points2D32f = (CvPoint2D32f *) malloc(npoints * sizeof(CvPoint2D32f));
    
    cvSetImageROI(grey[0], haar_rect2[0]);
    npoints = FindTrackPoints( grey[0], points2D32f, npoints, cp);
    cvResetImageROI(grey[0]);
    
    printf("goodfeaturestotrack %d\n", npoints);
    fflush(stdout);
    
    //crea las matrices, de momento con el tamaño máximo. Luego las reará
    CvMat *pointsI2 = cvCreateMat(npoints, 2, CV_32FC1);
    CvMat *pointsD2 = cvCreateMat(npoints, 2, CV_32FC1);
    CvMat *points3D2 = cvCreateMat(npoints, 3, CV_32FC1);

    for (i=0; i<npoints; i++)
    {
	cvmSet(pointsI2, i, 0, points2D32f[i].x);
	cvmSet(pointsI2, i, 1, points2D32f[i].y);
    }
    free(points2D32f);

    CvMat * mask = cvCreateMat(npoints, 1, CV_8UC1);
    FindStereoPoints(grey, haar_rect2, pointsI2, pointsD2, points3D2, mask, 
		     npoints, calL, calR, graficar);
    //busca los puntos que se salen del modelo
    int ipoints = cvCountNonZero(mask);
    printf("Puntos del modelo: %d.\n", ipoints);
    fflush(stdout);

    if (ipoints > 15)
    {
	RANSAC_circulo(20.0F, N_ITERATIONS_RANSAC_FIT_CIRCLE,  points3D2, mask);
	ipoints = cvCountNonZero(mask);
    }
    //cambia el tamaño de las matrices, para eliminar los puntos descartados
    *pointsI = cvCreateMat(ipoints, 2, CV_32FC1);
    *pointsD = cvCreateMat(ipoints, 2, CV_32FC1);
    *points3D = cvCreateMat(ipoints, 3, CV_32FC1);
    ipoints = 0;
    for (i=0; i<npoints; i++)
    {
	if (!cvGetReal1D(mask, i))
	    continue;
	cvmSet(*pointsI, ipoints, 0, cvmGet(pointsI2, i, 0));
	cvmSet(*pointsI, ipoints, 1, cvmGet(pointsI2, i, 1));
	cvmSet(*pointsD, ipoints, 0, cvmGet(pointsD2, i, 0));
	cvmSet(*pointsD, ipoints, 1, cvmGet(pointsD2, i, 1));
	cvmSet(*points3D, ipoints, 0, cvmGet(points3D2, i, 0));
	cvmSet(*points3D, ipoints, 1, cvmGet(points3D2, i, 1));
	cvmSet(*points3D, ipoints, 2, cvmGet(points3D2, i, 2));
	ipoints ++;
    }
    
    printf("Puntos del modelo: %d.\n", ipoints);

    *angY = cvCreateMat(ipoints, 2, CV_32FC1);
    //     cvSet(*angY, cvScalar(70.0*CV_PI/180.0));
    //     float tdata[] = {1.0F, 0.0F, 0.0F, -1.0F};
    //     CvMat tdatamat = cvMat(2, 2, CV_32FC1, tdata);
    //     cvMatMul(*angY, &tdatamat, *angY);
    GetAngles(*points3D, *angY);

    cvReleaseMat(&pointsI2);
    cvReleaseMat(&pointsD2);
    cvReleaseMat(&points3D2);
    
    cvReleaseImage(&grey[0]);
    cvReleaseImage(&grey[1]);
    cvReleaseMat(&mask);

    return ipoints;
}


int FindStereoPoints(IplImage ** grey, CvRect *roi, CvMat *pointsI, CvMat *pointsD, 
		     CvMat * XYZ, CvMat *maskout, 
		     int npoints, CALIBRATION_PARAMS_t * calL, CALIBRATION_PARAMS_t* calR, 
		     bool graficar)
{
    int i, method = 5;
    CvMat *lines;
    IplImage * n_mask;
    int methods[6] = { CV_TM_SQDIFF, CV_TM_SQDIFF_NORMED, CV_TM_CCORR, 
		       CV_TM_CCORR_NORMED, CV_TM_CCOEFF, CV_TM_CCOEFF_NORMED};
    CvPoint minpos, maxpos, maxpos2;
    double min, max;
    IplImage *result;
    CvMat *XYZi = cvCreateMat(3,1,CV_32F);

    float ur, vr, ul, vl, pt1yfloat;
    CvPoint pt0, pt1, pt2;

    CvRect roiresult, roimatch, roipatch, roigraph;
    CvSize match2[] = {{50, 100}, {30, 30}, {10, 10}};
    CvSize match;

    CvMat * graph1d, *suma1d;
    IplImage * graph = NULL;
    int w = roi[1].width; //cvGetSize(grey[0]).width;
    int h = 100;
    IplImage *grey2[2];

    graph1d = cvCreateMat(1, w, CV_32FC1);
    suma1d = cvCreateMat(1, w, CV_32FC1);

    if (graficar)
    {
	grey2[0] = cvCloneImage(grey[0]);
	grey2[1] =cvCloneImage(grey[1]);
	
	graph = cvCreateImage(cvSize(cvGetSize(grey[0]).width, h*2+10), IPL_DEPTH_32F, 3);
	cvZero(graph);

	roigraph.y=0;
	roigraph.height = cvGetSize(grey[0]).height;
	roigraph.x = roi[1].x;
	roigraph.width = roi[1].width;
	cvSetImageROI(graph, roigraph);
	cvNamedWindow("graph");
	cvNamedWindow(windowgraph[0]);
	cvNamedWindow(windowgraph[1]);
    }
    
    lines = cvCreateMat(npoints, 3, CV_32FC1);

    result = cvCreateImage(cvGetSize(grey[1]), IPL_DEPTH_32F, 1);
    n_mask = cvCreateImage(cvGetSize(grey[1]), IPL_DEPTH_8U, 1);
    cvZero(result);
                
    cvComputeCorrespondEpilines(pointsI, 1, calL->Fundamental, lines);
    
    cvResetImageROI(grey[0]);
    cvResetImageROI(grey[1]);
    
    //se establece la zona de búsqueda dentro del aḿbito de la cara, según la ROI del harr-like
    roiresult.width = roi[1].width;
    roiresult.x = roi[1].x;

    cvZero(maskout);

    bool mostrar_epi_lineas_poco_a_poco=false;
    if (0)
    {
	cvNamedWindow("h1");
	cvNamedWindow("h2");
	mostrar_epi_lineas_poco_a_poco=true;
    }

    for (i=0; i<npoints; i++)
    {
	//array acumulador de los resultados de las distintas correlaciones
	cvZero(suma1d);
	
	pt0.x = (int)cvmGet(pointsI, i, 0);
	pt0.y = (int)cvmGet(pointsI, i, 1);

	//linea para la máscara
	pt1.x = roi[1].x;
	pt2.x = roi[1].x + roi[1].width;

	pt1yfloat = -(float)cvmGet(lines, i, 2)/(float)cvmGet(lines, i, 1);
	pt1.y = (int)(pt1yfloat - (float)cvmGet(lines, i, 0)*pt1.x/(float)cvmGet(lines, i, 1));
	pt2.y = (int)(pt1yfloat - (float)cvmGet(lines, i, 0)*pt2.x/(float)cvmGet(lines, i, 1));

	cvSet(n_mask, cvScalar(255));
	cvLine(n_mask, pt1, pt2, cvScalar(0), 5, 8);

	if (mostrar_epi_lineas_poco_a_poco)
	{
	    int key;

	    cvLine(grey[1], pt1, pt2, cvScalar(250), 1, 8);
	    cvCircle (grey[0], pt0, 10, CV_RGB(200,200,200));
	    cvShowImage("h1", grey[0]);
	    cvShowImage("h2", grey[1]);
	    key = cvWaitKey(0);
	    key = key & (0x0FFFFF ^ 0x020000);
	    if (key == 'q' || key == 'Q')
		exit(-1);
	}

	//ROI para el resultado de la convolución. 
	//se busca alrededor de la línea epipolar
	if (pt1.y <= pt2.y)
	{
	    roiresult.y = pt1.y-2;
	    roiresult.height = pt2.y+2 - roiresult.y;
	}
	else 
	{
	    roiresult.y = pt2.y-2;
	    roiresult.height = pt1.y+2+1 - roiresult.y;
	}
	
	if (graficar)
	{
	    cvSetImageROI(graph, roigraph);
	    cvZero(graph);
	}

	//se eligen varios tamaños de parche
	unsigned int npatches = sizeof(match2)/sizeof(match2[0]);
	for (unsigned int m=0; m<npatches; m++)
	{
	    match = match2[m];
	    cvZero(result);
	    cvZero(graph1d);
	
	    //selección del parche a buscar
	    roipatch = cvRect(pt0.x-match.width/2, pt0.y-match.height/2, match.width, match.height);


	    // matching
	    // selección de la ventana de matching. parche en donde buscar
	    roimatch.width = roipatch.width + roiresult.width -1;
	    roimatch.height = roipatch.height + roiresult.height -1;
	    roimatch.x = roiresult.x - (int)roipatch.width/2;
	    roimatch.y = roiresult.y - (int)roipatch.height/2;
	    if (roimatch.y<0)
	    {
		break;
		//roimatch.height  -= -roimatch.y;
		//roiresult.height -= -roimatch.y;
		//roiresult.y += -roimatch.y;
		//roimatch.y += -roimatch.y;
		
	    }
	    //selección del parche a buscar (input)
	    cvSetImageROI(grey[0], roipatch);
	    // selección de las venetanas de búsqueda
	    cvSetImageROI(grey[1], roimatch);
	    cvSetImageROI(n_mask, roiresult);
	    cvSetImageROI(result, roiresult);
	
	    cvMatchTemplate(grey[1], grey[0], result, methods[method]);
	
	    // se sacan los valores de la linea epipolar de la correlación
	    cvSet(result, cvScalar(0), n_mask); 
	    cvReduce(result, graph1d, 0, CV_REDUCE_MAX);
	    //grafica(result, graph, graph1d, mask, cvmGet(lines,i,0), cvmGet(lines,i,1), cvmGet(lines,i,2), m);
	    // se acumula el valor con las otras correlaciones
	    cvAdd(suma1d, graph1d, suma1d);
	
	    cvResetImageROI(grey[0]);
	    cvResetImageROI(grey[1]);
	    cvResetImageROI(n_mask);
	    cvResetImageROI(result);

	    if (graficar)
	    {
		grafica(graph, graph1d, m);
	    }
	}
	
	if (roimatch.y<0)
	{
	    printf("Point %d not valid. Out of range\n",i );
	    //el punto no es válido
	    continue;
	}

	//para encontrar el punto exacto, busca en máximo de la suma de las correspondencias
	// de los tres parches. Y entorno a ese máximo, busca el máximo del parche más pequeño.
	//búsqueda del máximo de los tres parches
	cvMinMaxLoc(suma1d, &min, &max, &minpos, &maxpos);
	// si se usa uno de los métodos de CV_TM_SQDIFF o CV_TM_SQDIFF_NORMED, se buscar el mínimo
	if (method<=1)
	{
	    maxpos = minpos; 
	    max = 1-min;
	}
	
	//evalua que el valor de correlación encontrado sea superior a un umbral
	if (max < MIN_CORR_VALUE*(float)npatches || maxpos.x < 10)
	{
	    printf("Point %d not valid (value %f en %d).\n", i, max/(float)npatches, maxpos.x);
	    //el punto no es válido
	    continue;
	}

	//ahora busca el máximo el parche más pequeño, en torno al punto indicado.
	// graph1d ya contiene la correlación del parche más pequeño
	CvMat subgraph1d;
	if (i == 24)
	    ur =0;
	if (maxpos.x + 10 >= graph1d->width) 
	    maxpos.x = graph1d->width-11;
	if (maxpos.x < 10)
	    maxpos.x = 10;
	cvGetCols(graph1d, &subgraph1d, maxpos.x-10, maxpos.x+10+1);
	cvMinMaxLoc(&subgraph1d, &min, &max, &minpos, &maxpos2);
	if (method<=1) maxpos2 = minpos; 
	maxpos.x += maxpos2.x-10;

		    
	//la búsqueda se hizo con una ROI definida.
	//maxpos.x += roiresult.x;
	//maxpos.y += roiresult.y;
	
		
	//obtención del punto 3D correspondiente
	ur = (float)maxpos.x + roiresult.x;
	vr = pt1yfloat - (float)cvmGet(lines, i, 0)*ur/(float)cvmGet(lines, i, 1);
	
	ul = (float)cvmGet(pointsI, i, 0);
	vl = (float)cvmGet(pointsI, i, 1);

	GetStereoPoint(cvPoint2D32f(ul, vl), cvPoint2D32f(ur, vr), calL, calR, XYZi);

	cvmSet(XYZ, i, 0, cvmGet(XYZi, 0, 0));
	cvmSet(XYZ, i, 1, cvmGet(XYZi, 1, 0));
	cvmSet(XYZ, i, 2, cvmGet(XYZi, 2, 0));

	cvmSet(pointsD, i, 0, ur);
	cvmSet(pointsD, i, 1, vr);
	cvmSet(pointsI, i, 0, ul);
	cvmSet(pointsI, i, 1, vl);

	cvSetReal1D(maskout, i, 1);

	printf("%3d: %7.1f  %7.1f  %7.1f -> %f\n", i,  
	       cvmGet(XYZi, 0,0), cvmGet(XYZi,1,0), cvmGet(XYZi,2,0), max/(float)npatches);

	if (graficar)
	{
	    cvScale(suma1d, suma1d, 1.0/(float)npatches);
	    cvLine(graph, cvPoint(maxpos.x, 0), cvPoint(maxpos.x, h-1), CV_RGB(100,100,100), 1);
	    grafica(graph, suma1d, 3);

	    printf("grafica %d:\n", i);

	    cvResetImageROI(graph);
	    cvShowImage("graph", graph);
	    
	    cvCopy(grey[0], grey2[0]);
	    cvCopy(grey[1], grey2[1]);

	    cvCircle(grey2[0], cvPoint((int)ul, (int)vl), 10, CV_RGB(0,0,0), 1);
	    cvCircle(grey2[1], cvPoint((int)ur, (int)vr), 10, CV_RGB(0,0,0), 1);
	    cvShowImage(windowgraph[0], grey2[0]);
	    cvShowImage(windowgraph[1], grey2[1]);
	    /*
	      if (0)
	      {
	      cvNamedWindow("graph2");
	      cvShowImage("graph2", img);
	      cvNamedWindow("graph3");
	      cvShowImage("graph3", img2);
	      }
	    */

	    while(1)
	    {
		int  key = cvWaitKey(0) ;
		key = key & (0x0FFFFF ^ 0x020000);

		if(key == 'n' || key == 'n'){
		    break;
		}
		else if(key == ' ' ){
		    break;
		}
		else if(key == 'q' || key=='Q' ){
		    graficar=0;
		    break;
		}
	    }
	}


    }
    
    cvDestroyWindow("graph");
    cvDestroyWindow(windowgraph[0]);
    cvDestroyWindow(windowgraph[1]);
    cvReleaseImage(&graph);

    if (graficar)
    {
	cvReleaseImage(&grey2[0]);
	cvReleaseImage(&grey2[1]);
    }

    return 1;

}


int FindTrackPoints(IplImage *image, CvPoint2D32f *puntos, int n, struct corners_params_t *cp)
{
    int use_harris=1;
    double quality_level = cp->quality_level_max;
    //double k=0.2375;    
    //double min_distance = 30;

    CvRect roi = cvGetImageROI(image);
    cvResetImageROI(image);

    IplImage * eig_image = cvCreateImage(cvGetSize(image ), IPL_DEPTH_32F, 1);
    IplImage * temp_image = cvCloneImage(eig_image);

    IplImage* mask=cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);

    cvZero(mask);
    cvSetImageROI(mask, roi);
    cvSet(mask, cvScalar(255));
    cvResetImageROI(mask);
    
    int n_new=0;
    while(n_new <n-5 && quality_level >=cp->quality_level_min  )
    {
	n_new = n;
	cvGoodFeaturesToTrack(image, eig_image, temp_image, puntos, &n_new, 
			      quality_level, cp->min_distance, mask, 
			      cp->block_size, use_harris, cp->k_harris);
	printf("quality_level: %lf -> %d of %d\n", quality_level, n_new, n);
	fflush(stdout);
	quality_level /= 1.3;
    }
    
    n_new = 24;

    cvReleaseImage(&eig_image);
    cvReleaseImage(&temp_image);
    cvReleaseImage(&mask);

    return n_new;
}


void GetStereoPoint(CvMat *  pl, CvMat * pr, 
		    CALIBRATION_PARAMS_t *calL, CALIBRATION_PARAMS_t *calR, CvMat *points3D)
{
    int i;
    CvMat * XYZi = cvCreateMat(3,1,CV_32FC1);

    for (i=0; i<pl->rows && i<pr->rows; i++)
    {
	GetStereoPoint(((CvPoint2D32f*)pl->data.fl)[i], 
		       ((CvPoint2D32f*)pr->data.fl)[i], 
		       calL, calR, XYZi);
	cvmSet(points3D, i, 0, cvmGet(XYZi, 0, 0));
	cvmSet(points3D, i, 1, cvmGet(XYZi, 1, 0));
	cvmSet(points3D, i, 2, cvmGet(XYZi, 2, 0));
    }

}

void GetStereoPoint(CvPoint2D32f pl, CvPoint2D32f pr, 
		    CALIBRATION_PARAMS_t *calL, CALIBRATION_PARAMS_t *calR, CvMat *point3D)
{

    STEREO_PARAMS_t *tmp = calL->tmp;
    //obtención del punto 3D correspondiente
    cvmSet(tmp->M_A, 0,2, -(pl.x - calL->kk[0*3+2]) );
    cvmSet(tmp->M_A, 1,2, -(pl.y - calL->kk[1*3+2]) );
	
    cvmSet(tmp->M_A, 2,0, tmp->rr[2*3+0] * (pr.x - calR->kk[0*3+2]) - tmp->fxR00);
    cvmSet(tmp->M_A, 2,1, tmp->rr[2*3+1] * (pr.x - calR->kk[0*3+2]) - tmp->fxR01);
    cvmSet(tmp->M_A, 2,2, tmp->rr[2*3+2] * (pr.x - calR->kk[0*3+2]) - tmp->fxR02);
	
    cvmSet(tmp->M_A, 3,0, tmp->rr[2*3+0] * (pr.y - calR->kk[1*3+2]) - tmp->fyR10);
    cvmSet(tmp->M_A, 3,1, tmp->rr[2*3+1] * (pr.y - calR->kk[1*3+2]) - tmp->fyR11);
    cvmSet(tmp->M_A, 3,2, tmp->rr[2*3+2] * (pr.y - calR->kk[1*3+2]) - tmp->fyR12);
	
    cvmSet(tmp->V_B, 2,0, tmp->fxT0 - tmp->tt[2]*(pr.x - calR->kk[0*3+2]));
    cvmSet(tmp->V_B, 3,0, tmp->fyT1 - tmp->tt[2]*(pr.y - calR->kk[1*3+2]));
	
    cvSolve(tmp->M_A, tmp->V_B, point3D, CV_SVD);

    //cvMatMulAdd(calL->R, point3D, calL->T, point3D);
    //printf("%f, %f, %f\n", cvmGet(point3D, 0,0), cvmGet(point3D, 1,0), cvmGet(point3D, 2,0));
    //fflush(stdout);
	
}

void grafica(IplImage * graph, CvMat *graph1d, int icolor)
{

    int w = graph1d->cols;
    int h = (cvGetSize(graph).height-10)/2;

    CvScalar color;
    int thick;

    switch (icolor)
    {
    case 0:
	color = CV_RGB(0, 0, 255);
	thick = 1;
	break;
    case 1:
	color = CV_RGB(0, 255, 0);
	thick = 1;
	break;
    case 2:
	color = CV_RGB(255, 0, 0);
	thick = 1;
	break;
    default:
	color = CV_RGB(255, 255, 255);
	thick = 1;
	break;
    }

    int j;
    int jlast = h+5;
    for (int i=0; i<w; i++)
    {
	j = (int)(h-cvmGet(graph1d, 0, i)*(float)h+5);
	//	cvSet2D(graph, j, i, color);
	cvLine(graph, cvPoint(i-1, jlast), cvPoint(i, j), color, thick, 8);
	jlast = j;
    }
}



/* función que aplica un RANSAC sobre las coordenadas (x,z) del modelo para ajustar un círculo a los puntos del modelo.
   Devuelve el centro del círculo (x, y) y el radio (en z), y una máscara con los outliers a '0'


   ITeraciones del RANSAC.
   Va cogiendo puntos de 3 en 3, y calcula la circunferencia que pasa por esos 3 puntos.
   Después mira las distancias de dicha circunferencia al resto de puntos, y
   se queda con la circunferencia que tanga mas inliers

*/
CvPoint3D32f  RANSAC_circulo (float umbral, int iterations, CvMat *points3D, CvMat *mask)
{
    printf("inicio Ransac círculo\n"); fflush(stdout);
    int npoints = points3D->rows;
    
    int p1, p2, p3;
    float y1, y2, y3;
    float x1, x2, x3;
    int inliers;
    float error;

    int maxinliers=0;
    int pmax[3];
    CvPoint3D32f cmax = {0, 0, 0};
    float minerror = 1.0e8;
    
    CvMat d3sub;
    double xmax, xmin, ymax, ymin;

    CvMat * masktemp =cvCreateMat(npoints, 1, CV_8UC1);
    CvMat * maskout  =cvCreateMat(npoints, 1, CV_8UC1);

    //selección de la columna X para buscar el máximo y el mínimo en X
    cvGetCol(points3D, &d3sub, 0);
    cvMinMaxLoc(&d3sub, &xmin, &xmax);
    //selección de la columna Z para buscar el mínimo en Y
    cvGetCol(points3D, &d3sub, 2);
    cvMinMaxLoc(&d3sub, &ymin, &ymax);

    //srand( (unsigned)time( NULL ) );
    srand(1);  //siempre la misma secuencia aleatoria. Debug


    /* ITeraciones del RANSAC.
       Va cogiendo puntos de 3 en 3, y calcula la circunferencia que pasa por esos 3 puntos.
       Después mira las distancias de dicha circunferencia al resto de puntos, y
       se queda con la circunferencia que tanga mas inliers
    */

    int iter=0, iter2=0;
    while (iter < iterations && iter2++ < N_ITERATIONS_RANSAC_FIT_CIRCLE_MAX )
    {
	// selección de los puntos a usar para calcular el círculo
	do {
	    p1 = rand() % npoints; //coge un punto aleatorio
	} while ( !cvGetReal1D(mask, p1));
	do {
	    p2 = rand() % npoints; //coge un punto aleatorio
	} while ( !cvGetReal1D(mask, p2));
	do {
	    p3 = rand() % npoints; //coge un punto aleatorio
	} while ( !cvGetReal1D(mask, p3));

	if (p2 == p1 ||  p2==p3 || p1==p3)
	    continue;
		
	y1 = cvmGet(points3D, p1, 2); //toma la coordenada Z para el proyectar sobre el plano Y
	y2 = cvmGet(points3D, p2, 2); //toma la coordenada Z para el proyectar sobre el plano Y
	y3 = cvmGet(points3D, p3, 2); //toma la coordenada Z para el proyectar sobre el plano Y
	x1 = cvmGet(points3D, p1, 0); //toma la coordenada X para el proyectar sobre el plano X
	x2 = cvmGet(points3D, p2, 0); //toma la coordenada X para el proyectar sobre el plano X
	x3 = cvmGet(points3D, p3, 0); //toma la coordenada X para el proyectar sobre el plano X

	//Si alguno de los puntos está alineado con el otro, el centro no se puede calcular 
	//siguiendo este método sencillo
	if ((y2-y1<1.0F && y2-y1>-1.0F) || (y2-y3<1.0F && y2-y3>-1.0F))
	    continue;

	//calculo del centro del circulo que pasa por los tre puntos seleccionados
	float ap1 = (x1-x2)/(y2-y1);
	float bp1 = (y2+y1)/2 - ap1*(x2+x1)/2;
	float ap2 = (x3-x2)/(y2-y3);
	float bp2 = (y2+y3)/2 - ap2*(x2+x3)/2;
	if (ap2 - ap1 < 1.0F && ap2 - ap1 > -1.0F)
	    continue;

	float x = (bp1 - bp2)/(ap2-ap1);
	if (x>xmax || x<xmin)
	    continue;
	float y = ap2*x+bp2;
	if (y<ymin)
	    continue;
	double r = sqrt(pow(x3-x,2.0F) + pow(y3-y, 2.0F));

	//calculo de las distancias de cada punto a la circunferencia.
	// calculo del número de inliers y del la suma del error de inliers
	inliers = 0;
	error = 0;
	cvZero(masktemp);

	if (p1 == 17 && p2 == 6 && p3 == 23)
	{
	    inliers = 0;
	    error = 0;
	    cvZero(masktemp);
	}
	    
	for (int i =0; i<npoints; i++)
	{
	    float d;
	    if (!cvGetReal1D(mask, i))
		continue;
	    if (cvmGet(points3D, i, 2)>=y)
		continue;
	    d = sqrt(pow(cvmGet(points3D, i, 0) - x, 2) + pow(cvmGet(points3D, i, 2) - y, 2)) - r;
	    if (d<=umbral && d>= -umbral)
	    {
		inliers++;
		error += d*d;
		cvSetReal1D(masktemp, i, 1);
	    }
	}
	if (inliers <20)
	    continue;
	error = sqrt(error)/(float)inliers;
	
	if (inliers > maxinliers || (inliers == maxinliers && error < minerror))
	{
	    maxinliers = inliers;
	    pmax[0] = p1;
	    pmax[1] = p2;
	    pmax[2] = p3;
	    cmax.x = x;
	    cmax.y = y;
	    cmax.z = r;
	    minerror = error;
	    cvCopy (masktemp, maskout);
	}

	iter ++;
    }

    cvCopy(maskout, mask);
    cvReleaseMat(&masktemp);
    cvReleaseMat(&maskout);

    return  cmax;
    printf("fin Ransac círculo\n"); fflush(stdout);
}



/*función que calcula los ángulos de ocultamiento de cada punto del modelo

  Aplica un RANSAC para calcular la circunferencia que más se ajusta al modelo de 
  cara

  Devuelve los ángulos límites de giro del modelo para los que cada punto se oculta.
*/

void GetAngles(CvMat *points3D, CvMat *angY)
{
    int npoints = points3D->rows;
    CvMat * mask = cvCreateMat(npoints, 1, CV_8UC1);
    cvSet(mask, cvScalar(1));
    //llama al RANSAC para ajustar el círculo
    CvPoint3D32f circle = RANSAC_circulo(15.0F, N_ITERATIONS_RANSAC_FIT_CIRCLE, points3D, mask);
    // en circle.(x, y) está el centro de la circunferencia, y 
    // circle.z es el radio
    float angle;
    float angle_max, angle_min;

    //cálculo de los ángulos
    for (int i=0; i<npoints; i++)
    {
	angle = atan( (cvmGet(points3D,i,0)-circle.x) / (cvmGet(points3D,i,2)-circle.y) );
	//if (angle > 0)
	{
	    angle_max = LIMIT_ANGLE_Y - angle;
	    angle_min = -LIMIT_ANGLE_Y - angle;
	    if (angle_max < MIN_ANGLE_Y)
		angle_max = MIN_ANGLE_Y;
	    else if (angle_max > MAX_ANGLE_Y)
		angle_max = MAX_ANGLE_Y;

	    if (angle_min > -MIN_ANGLE_Y)
		angle_min = -MIN_ANGLE_Y;
	    else if (angle_min < -MAX_ANGLE_Y)
		angle_min = -MAX_ANGLE_Y;
	}
	cvmSet(angY, i, 0, angle_max);
	cvmSet(angY, i, 1, angle_min);
    }
    cvReleaseMat(&mask);
}


CvPoint trackpoint(CvPoint pt1, IplImage *img, CvPoint pt0, IplImage * oldimg)
{
    int method = 5;
    int methods[6] = { CV_TM_SQDIFF, CV_TM_SQDIFF_NORMED, CV_TM_CCORR, 
		       CV_TM_CCORR_NORMED, CV_TM_CCOEFF, CV_TM_CCOEFF_NORMED};
    CvPoint minpos, maxpos;
    double min, max;
    IplImage *result;
    CvMat *suma;

    CvRect roimatch, roipatch;
    CvSize match2[] = {{51,51}, {25, 25}, {11, 11}};
    CvSize match;

    //tamaño de la zona de búsqueda. Tamaño del resultado
    int w = 61;
    int h = 61;

    result = cvCreateImage(cvSize(w, h), IPL_DEPTH_32F, 1);
    suma  = cvCreateMat(h, w, CV_32FC1);
    cvZero(suma);
    
    //se eligen varios tamaños de parche
    unsigned int npatches = sizeof(match2)/sizeof(match2[0]);
    for (unsigned int m=0; m<npatches; m++)
    {
	match = match2[m];
	cvZero(result);
	
	//selección del parche a buscar
	roipatch = cvRect(pt0.x-match.width/2, pt0.y-match.height/2, match.width, match.height);
	// matching
	// selección de la ventana de matching. parche en donde buscar
	roimatch.width = roipatch.width + w -1;
	roimatch.height = roipatch.height + h -1;
	roimatch.x = pt1.x - w/2 - (int)roipatch.width/2;
	roimatch.y = pt1.y - h/2 - (int)roipatch.height/2;
	if (roimatch.y<0 || roimatch.x<0)
	{
	    break;
	}
	//selección del parche a buscar (input)
	cvSetImageROI(oldimg, roipatch);
	// selección de las venetanas de búsqueda
	cvSetImageROI(img, roimatch);
	
	cvMatchTemplate(img, oldimg, result, methods[method]);
	
	cvAdd(suma, result, suma);
	
	cvResetImageROI(img);
	cvResetImageROI(oldimg);
    }
	
    if (roimatch.y<0 || roimatch.x<0)
    {
	cvReleaseImage(&result);
	cvReleaseMat(&suma);
	printf("Point not valid. Out of range\n" );
	//el punto no es válido
	return cvPoint(-1, -1);
    }
	
    //para encontrar el punto exacto, busca en máximo de la suma de las correspondencias
    // de los tres parches. Y entorno a ese máximo, busca el máximo del parche más pequeño.
    //búsqueda del máximo de los tres parches
    cvMinMaxLoc(suma, &min, &max, &minpos, &maxpos);
    // si se usa uno de los métodos de CV_TM_SQDIFF o CV_TM_SQDIFF_NORMED, se buscar el mínimo
    if (method<=1)
    {
	maxpos = minpos; 
	max = 1-min;
    }
	
    //evalua que el valor de correlación encontrado sea superior a un umbral
    if (max < MIN_CORR_VALUE*(float)npatches )
    {
	//el punto no es válido
	cvReleaseImage(&result);
	cvReleaseMat(&suma);
	return cvPoint(-1, -1);
    }

    //ahora busca el máximo el parche más pequeño, en torno al punto indicado.
    // result ya contiene la correlación del parche más pequeño
    if (maxpos.x + 10 >= w) 
	maxpos.x = w-11;
    if (maxpos.x < 10)
	maxpos.x = 10;
    if (maxpos.y + 10 >= h) 
	maxpos.y = h-11;
    if (maxpos.y < 10)
	maxpos.y = 10;

    cvSetImageROI(result, cvRect(maxpos.x-5, maxpos.y-5, 10, 10));
    cvMinMaxLoc(result, &min, &max, &minpos, &maxpos);
    if (method<=1) maxpos = minpos; 

    maxpos.x += result->roi->xOffset - w/2 + pt1.x;
    maxpos.y += result->roi->yOffset - h/2 + pt1.y;

    cvReleaseImage(&result);
    cvReleaseMat(&suma);

    return maxpos;
}



